#
# sudo nano /home/cmh/.bashrc
# 配置为登录启动，但是,配置sudo nano /etc/rc.local开机启动无效
# irremote 32 gpio12 配置在系统中/boot/config.txt
#
import evdev
import RPi.GPIO as GPIO
import time

LEFT_LED = 7  # gpio 7
RIGHT_LED = 40   # gpio 21

# set GPIO.BOARD Pins
# motor
Left_Black  = 11  # IN1 GPIO17
Left_Red = 12     # IN2  GPIO18 PCM_CLK
Right_Black  = 13  #IN3 GPIO 27
Right_Red = 15    # IN4 GPIO 22
# ultrasonic
TRIGGER = 31  # pin6
ECHO = 29   # pin5

MOTOR_STATE = None


def get_ir_device():
    devices = [evdev.InputDevice(path) for path in evdev.list_devices()]
    for device in devices:
        if (device.name == "gpio_ir_recv"):
            return device

def motor_turn_left():
    GPIO.output(Left_Red, False)
    GPIO.output(Left_Black, False)
    GPIO.output(Right_Red, GPIO.HIGH)
    GPIO.output(Right_Black, GPIO.LOW)
    MOTOR_STATE = "TURN_LEFT"


def motor_turn_right():
    GPIO.output(Left_Red, GPIO.HIGH)
    GPIO.output(Left_Black, GPIO.LOW)
    GPIO.output(Right_Red, False)
    GPIO.output(Right_Black, False)
    MOTOR_STATE = "TURN_RIGHT"


def motor_go():
    GPIO.output(Left_Red, GPIO.HIGH)
    GPIO.output(Left_Black, GPIO.LOW)
    GPIO.output(Right_Red, GPIO.HIGH)
    GPIO.output(Right_Black, GPIO.LOW)
    MOTOR_STATE = "GO"


def motor_stop():
    GPIO.output(Left_Red, GPIO.LOW)
    GPIO.output(Left_Black, GPIO.LOW)
    GPIO.output(Right_Red, GPIO.LOW)
    GPIO.output(Right_Black, GPIO.LOW)
    MOTOR_STATE = "STOP"


def motor_back():
    GPIO.output(Left_Red, GPIO.LOW)
    GPIO.output(Left_Black, GPIO.HIGH)
    GPIO.output(Right_Red, GPIO.LOW)
    GPIO.output(Right_Black, GPIO.HIGH)
    MOTOR_STATE = "BACK"


def distance():
    # set Trigger to HIGH
    GPIO.output(TRIGGER, True)

    # set Trigger after 0.01ms to LOW
    time.sleep(0.00001)
    GPIO.output(TRIGGER, False)

    StartTime = time.time()
    StopTime = time.time()

    # save StartTime
    while GPIO.input(ECHO) == 0:
        StartTime = time.time()

    # save time of arrival
    while GPIO.input(ECHO) == 1:
        StopTime = time.time()

    # time difference between start and arrival
    TimeElapsed = StopTime - StartTime
    # multiply with the sonic speed (34300 cm/s)
    # and divide by 2, because there and back
    distance = (TimeElapsed * 34300) / 2

    return distance

def setup_gpio():
    GPIO.setwarnings(False)
    GPIO.setmode(GPIO.BOARD)
    GPIO.cleanup()
      # motor
    GPIO.setup(Left_Black, GPIO.OUT)
    GPIO.setup(Left_Red, GPIO.OUT)
    GPIO.setup(Right_Black, GPIO.OUT)
    GPIO.setup(Right_Red, GPIO.OUT)
    motor_stop()

    # ultrasonic
    GPIO.setup(TRIGGER, GPIO.OUT)
    GPIO.setup(ECHO, GPIO.IN)
    # led    
    GPIO.setup(LEFT_LED, GPIO.OUT)
    GPIO.setup(RIGHT_LED, GPIO.OUT)

if __name__ == '__main__':
    setup_gpio()
    device = get_ir_device()
    # 标准程序运行中
    for i in range(5):
        GPIO.output(LEFT_LED, GPIO.LOW)
        GPIO.output(RIGHT_LED, GPIO.LOW)
        time.sleep(0.2)
        GPIO.output(LEFT_LED, GPIO.HIGH)
        GPIO.output(RIGHT_LED, GPIO.HIGH)
        time.sleep(0.2)
    try:
        while True:
            time.sleep(0.05)
            dist = distance()
            print ("Measured Distance = %.1f cm" % dist)
            if (dist < 30 ):
                motor_stop()
            next_event =  device.read_one()
            if (next_event):
                if (next_event.value==24): # 2
                    GPIO.output(LEFT_LED, GPIO.HIGH)
                    GPIO.output(RIGHT_LED, GPIO.HIGH)
                    motor_go()
                elif (next_event.value==8):
                    GPIO.output(LEFT_LED, GPIO.HIGH)
                    GPIO.output(RIGHT_LED, GPIO.LOW)
                    motor_turn_right()
                elif (next_event.value==90):
                    GPIO.output(LEFT_LED, GPIO.LOW)
                    GPIO.output(RIGHT_LED, GPIO.HIGH)
                    motor_turn_left()
                elif (next_event.value==82): # 8
                    GPIO.output(LEFT_LED, GPIO.LOW)
                    GPIO.output(RIGHT_LED, GPIO.LOW)
                    motor_back()
                elif (next_event.value==28): # 5
                    GPIO.output(LEFT_LED, GPIO.LOW)
                    GPIO.output(RIGHT_LED, GPIO.LOW)
                    motor_stop()    
       
    except KeyboardInterrupt:  # Ctrl+C
        GPIO.cleanup()
